


//头文件
#include <ros/ros.h>
#include <iostream>
//#include <Eigen/Eigen>

#include <vector>

//msg头文件
#include <geometry_msgs/PoseStamped.h>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <nav_msgs/Odometry.h>


ros::Publisher camera_pose_pub;
//camera_pose odometry
Eigen::Vector3d px4_camera_pose_vins;
Eigen::Quaterniond px4_camera_q_vins;
Eigen::Vector3d px4_camera_euler_vins;


//回调，获取vins估计的camera_pose
void vins_camera_cb(const nav_msgs::Odometry::ConstPtr &msg)
{
    px4_camera_pose_vins = Eigen::Vector3d(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
    px4_camera_q_vins = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
    //px4_camera_euler_vins = quaternion_to_euler(px4_camera_q_vins);
}

void send_to_planner()
{
    geometry_msgs::PoseStamped camera_pose;
    camera_pose.header.frame_id = "world";
    //使用vins定位
 
	camera_pose.pose.position.x = px4_camera_pose_vins[0];
	camera_pose.pose.position.y = px4_camera_pose_vins[1];
	camera_pose.pose.position.z = px4_camera_pose_vins[2];

	camera_pose.pose.orientation.x = px4_camera_q_vins.x();
	camera_pose.pose.orientation.y = px4_camera_q_vins.y();
	camera_pose.pose.orientation.z = px4_camera_q_vins.z();
	camera_pose.pose.orientation.w = px4_camera_q_vins.w();


    camera_pose.header.stamp = ros::Time::now();
    camera_pose_pub.publish(camera_pose);

}




int main(int argc, char **argv)
{

    
    ros::init(argc, argv, "odomToPose");
    ros::NodeHandle nh("~");
    
    ros::Subscriber vins_camera_sub = nh.subscribe<nav_msgs::Odometry>("/vins_estimator/camera_pose", 5, vins_camera_cb);
    
    // 【发布】vins估计的相机姿态给fastplanner
    camera_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/px4/vins/camera_pose", 5);
  
	ros::Rate rate(10.0);

    //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Main Loop<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while(ros::ok())
    {
        //回调一次 更新传感器状态
        ros::spinOnce();

        //发布相机位姿到fastplanner
        send_to_planner();
        
        rate.sleep();
    // 频率
    }
    return 0;
}
